#include "search.h"
#include <math.h>
#if(KEIL)
#include "SEEKFREE_MT9V03X.h"
#endif


int IMAGE_MIDDLE = 130;
int CircleNeedRun = 1;  // 运行圈数
int g_speed_carage = 0;  // 车库速度
/********************************************变量声明***************************************************/
uint8        g_VideoImageData[MAX_VIDEO_LINE][MAX_VIDEO_POINT];  //图像原始数据，g_VideoImageData[m][n]代表图像的从上数第m行，从左数第n点，0为最黑，255为最白
//==============================图像数据================================//
_POINT  g_CenterPosition[MAX_POINT_NUM];
_POINT  g_LeftEdge[MAX_POINT_NUM], g_RightEdge[MAX_POINT_NUM];
int          g_CenterNum;
int          g_LeftEdgeNum, g_RightEdgeNum;
//==============================图像处理用变量==========================//
uint16       g_Start[10], g_End[10], g_SEnum;
uint8        g_SearchFlag = 1;
int          g_Cover;
uint8        g_CoverIndex[10], g_CoverNum;
//==============================控制用变量==============================//
int			g_DirectionControl = MID_VIDEO_POINT;
int			g_FormerDirectionControl = MID_VIDEO_POINT;
int			g_DirectionControlWhole = 0;
int			g_DirectionControlLine = 0;


//==============================新添的变量==============================//
bool FlagStraightLine = 0;  // 直道的标志，默认不是
bool FlagCarage = 0;  // 车库标志，默认不是
bool FlagOnlyLeftEdge = 0, FlagOnlyRightEdge = 0;  // 只有左边线，只有右边线
bool FlagLeftToCarage = 0, FlagRightToCarage = 0;  // 左右入库
bool FlagLeftOutCarage = 0, FlagRightOutCarage = 0;  // 左右出库
int g_LeftEdgeEndNum = 0;
int g_RightEdgeEndNum = 0;
bool FlagCross = 0;  // 十字
bool OutCarage = 1;  // 出库
bool InCarage = 0;  // 入库
_POINT CarageOutLeftPoint, CarageOutRightPoint;
int NumZbraBlockings = 0;  // 斑马线块数目
//======================================================================//

/*********	search:处理数据即测试算法的函数 ********/
void Search()
{
	InitVariate();  // 初始化变量

	IMAGE_MIDDLE_SET();  // 获取动态阈值，赛道反光特别严重时，阈值并不准确

	// 对图像进行滤波处理，去掉噪点

	CannySearch(); // 边线检索，获取元素类型

	// 根据元素类型获取中线位置
	if(FlagStraightLine)
	{
		StraightLine();
	}
	else
	{
		if (FlagOnlyLeftEdge)
		{
			OnlyOneEdge(0, g_LeftEdgeNum, 'L', 0);
		}
		else
		{
			if (FlagOnlyRightEdge)
			{
				OnlyOneEdge(0, g_RightEdgeNum, 'R', 0);
			}
			else 
			{
				 // 十字
				if (FlagCross)
				{
					StraightCross('l');
				}
				else
				{
					if (FlagCarage)  // 车库
					{
						if (OutCarage)
						{
							if (FlagLeftOutCarage) // 左出库
							{
								float raidus = (CarageOutRightPoint.y - CarageOutLeftPoint.y) / 2;
								float circle_x = CarageOutRightPoint.x;
								float circle_y = (CarageOutRightPoint.y + CarageOutLeftPoint.y) / 2;
								for (int i = CarageOutRightPoint.x; i < g_RightEdgeNum; i++)
								{
									g_RightEdge[i].x = i;
									g_RightEdge[i].y = INT16S(sqrt(raidus * raidus - (i - circle_y) * (i - circle_y)) + circle_y);
									g_LeftEdge[i].x = i;
									g_LeftEdge[i].y = 0;
									g_CenterNum += 1;
									g_CenterPosition[i].x = i;
									g_CenterPosition[i].y = INT16S(sqrt(raidus * raidus - (i - circle_y) * (i - circle_y)) + circle_y - raidus);
								}
								g_CenterNum += int(CarageOutRightPoint.x);
							}
							if (FlagRightOutCarage)  // 右出库
							{
								float raidus = (CarageOutRightPoint.y - CarageOutLeftPoint.y) / 2;
								float circle_x = CarageOutLeftPoint.x;
								float circle_y = (CarageOutRightPoint.y + CarageOutLeftPoint.y) / 2;
								for (int i = CarageOutLeftPoint.x; i < g_LeftEdgeNum; i++)
								{
									g_LeftEdge[i].x = i;
									g_LeftEdge[i].y = INT16S(sqrt(raidus * raidus - (i - circle_y) * (i - circle_y)) + circle_y);
									g_RightEdge[i].x = i;
									g_RightEdge[i].y = 161;
									g_CenterNum += 1;
									g_CenterPosition[i].x = i;
									g_CenterPosition[i].y = INT16S(sqrt(raidus * raidus - (i - circle_y) * (i - circle_y)) + circle_y + raidus);
								}
								g_CenterNum += int(CarageOutLeftPoint.x);
							}
						}
						if (InCarage)
						{
							if (FlagRightToCarage) // 右入库
							{

							}
							if (FlagLeftToCarage)  // 左入库
							{

							}
						}
					}
				}
			}
		}
	}
}

/*自适应动态阈值范围，获得一个相对最佳的阈值，但是容易受到光照不均匀的影响*/
void IMAGE_MIDDLE_SET()
{
	// 大津法
	unsigned long Amount = 0;
	unsigned long PixelBack = 0;
	unsigned long PixelshortegralBack = 0;
	unsigned long Pixelshortegral = 0;
	signed long PixelshortegralFore = 0;
	signed long PixelFore = 0;
	float OmegaBack, OmegaFore, MicroBack, MicroFore, SigmaB, Sigma; // 类间方差
	signed short MinValue = 0, MaxValue = 255;
	unsigned char HistoGram[256] = { 0 };
	for (int i = 0; i < MAX_VIDEO_LINE; i++)
		for (int j = 0; j < MAX_VIDEO_POINT; j++)
			HistoGram[g_VideoImageData[i][j]]++;
	for (; MinValue < 256 && HistoGram[MinValue] == 0; MinValue++);
	for (; MaxValue > MinValue && HistoGram[MaxValue] == 0 && MaxValue != MinValue; MaxValue--);
	if (MaxValue == MinValue)
	{
		IMAGE_MIDDLE = MaxValue;
	}
	else
	{
		if ((MinValue + 1) == MaxValue)
		{
			IMAGE_MIDDLE = MinValue;
		}
		else
		{
			Pixelshortegral = 0;
			SigmaB = -1;
			for (int i = MinValue; i <= MaxValue; i++)
			{
				Amount += HistoGram[i];  // 像素点总数
				Pixelshortegral += HistoGram[i] * i;  // 灰度值总数
			}
			float average_pixel = float(Pixelshortegral) / Amount;
			for (int i = MinValue; i <= MaxValue; i++)
			{
				PixelBack = PixelBack + HistoGram[i];  // 前景像素点数
				PixelFore = Amount - PixelBack;  // 背景像素点数
				OmegaBack = ((float)PixelBack) / Amount;  // 前景像素百分比
				OmegaFore = ((float)PixelFore) / Amount;  // 背景像素百分比
				PixelshortegralBack += HistoGram[i] * i; // 前景灰度值
				PixelshortegralFore = Pixelshortegral - PixelshortegralBack;  // 背景灰度值
				MicroBack = ((float)PixelshortegralBack) / PixelBack;  // 前景灰度百分比
				MicroFore = ((float)PixelshortegralFore) / PixelFore;  // 背景灰度百分比
				Sigma = PixelBack * (MicroBack - average_pixel) * (MicroBack - average_pixel) + PixelFore * (MicroFore - average_pixel) * (MicroFore - average_pixel);  // 类间方差
				if (Sigma > SigmaB)
				{
					SigmaB = Sigma;
					IMAGE_MIDDLE = i;
				}
			}
		}
	}
}

/*直道或者类直道中线,此时g_LeftEdgeNum==g_RightEdgeNum*/
void StraightLine()
{
	g_CenterNum = g_RightEdgeNum;
	for (int i = 0; i < g_CenterNum; i++)
	{
		g_CenterPosition[i].x = g_RightEdge[i].x;
		g_CenterPosition[i].y = (g_RightEdge[i].y + g_LeftEdge[i].y) / 2;
	}
}

/*方向控制,flag表示是否需要停车*/
void DirctionControl(bool flag)
{
	if (flag)
	{
		int g_speed_final = 0;
	}
	else
	{
		//===================控制中心及电机控制====================//
//加权平均法求方向控制
		g_DirectionControlWhole = 0;
		g_DirectionControlLine = 0;
		for (int i = 0; i < g_CenterNum; i++)
		{
			if (g_CenterPosition[i].y >= 0 && g_CenterPosition[i].y <= MAX_VIDEO_POINT)
			{
				g_DirectionControlLine += (int)g_CenterPosition[i].x;
				g_DirectionControlWhole += (int)g_CenterPosition[i].y * g_CenterPosition[i].x;  //注意数据不要溢出
			}
		}
		if (g_DirectionControlLine > 0)
		{
			g_DirectionControl = g_DirectionControlWhole / g_DirectionControlLine;
		}
		//方向控制限定
		if (g_DirectionControl < 0)
		{
			g_DirectionControl = 0;
		}
		else if (g_DirectionControl > MAX_VIDEO_POINT)
		{
			g_DirectionControl = MAX_VIDEO_POINT;
		}
		if (ABS(g_DirectionControl - g_FormerDirectionControl) > 90)
		{
			g_DirectionControl = g_FormerDirectionControl;
		}
	}
}

/*边线提取*/
void CannySearch()
{
	PreDipose();

	printf("预提取已经完成\n");
	// 提取图像特征
	// 类直道或直道,白块从第一行延伸到最后一行
	if (g_LeftEdgeNum == g_RightEdgeNum && g_LeftEdgeNum == MAX_VIDEO_LINE)
	{
		FlagStraightLine = 1;  // 直道
	}
	// 单边线
	if (g_LeftEdgeNum < 60 && g_RightEdgeNum < 60)
	{
		int LeftEdgeBreakLoop = 0;  // 左边线开始退出循环的行数
		int LoopVariate = 0;  // 循环变量
		int LeftLineCount = 0, RightLineCount = 0;  // 计数
		// 遍历左边线数组
		for (; LoopVariate < g_RightEdgeNum; LoopVariate++)
		{
			if (g_LeftEdge[LoopVariate].y >= 0 && g_RightEdge[LoopVariate].y == 161)  // 只有左线像素
			{
				OnlyOneEdge(0, 0, 'l', LoopVariate);
				LeftLineCount++;
			}
			if (g_LeftEdge[LoopVariate].y == 0 && g_RightEdge[LoopVariate].y <= 161)  // 只有右线像素
			{
				RightLineCount++;
				OnlyOneEdge(0, 0, 'r', LoopVariate);
			}
		}
		if (ABS(RightLineCount - g_RightEdgeNum) <= 2)  // 避免误差，选择小于2行，认为只有右边线存在
		{
			FlagOnlyRightEdge = 1;
			g_RightEdgeEndNum = g_RightEdgeNum;
		}
		if (ABS(LeftLineCount - g_RightEdgeNum) <= 2)  // 避免误差，选择小于2行，认为只有左边线存在
		{
			FlagOnlyLeftEdge = 1;
			g_LeftEdgeEndNum = g_LeftEdgeNum;
		}
		// 多于右边线的像素点，可以直接利用平移方法进行计算
		OnlyOneEdge(g_RightEdgeNum, g_LeftEdgeNum, 'L', 0);
	}
	// 斑马线以及车库特征提取
	int LeftFlag = 0, RightFlag = 0, count = 0;
	for (int i = 0; i < g_LeftEdgeNum-1; i++)
	{
		if (g_LeftEdge[i].y > 0 && g_LeftEdge[i + 1].y == 0 && LeftFlag == 0)
		{
			CarageOutLeftPoint.x = i;
			CarageOutLeftPoint.y = g_LeftEdge[i].y;
			LeftFlag = 1;
		}
		if (g_RightEdge[i].y < 161 && g_RightEdge[i + 1].y == 161 && RightFlag == 0)
		{
			CarageOutRightPoint.x = i;
			CarageOutRightPoint.y = g_RightEdge[i].y;
			RightFlag = 1;
		}
		if (ABS(g_LeftEdge[i].y - g_RightEdge[i].y) == 161)
		{
			count++;
		}
		if (LeftFlag && RightFlag)
			break;
	}
	// 出库
	if (count >= 10)
	{
		FlagCarage = 1;
		OutCarage = 1;
		FlagLeftOutCarage = 1;
	}
	if (NumZbraBlockings >= 8)
	{
		InCarage = 1;
		if (FlagLeftOutCarage)
		{
			FlagLeftToCarage = 1;
		}
		else
		{
			FlagRightToCarage = 1;
		}
	}
}

/*单边线,一边边线为161或者0*/
/*只有一组边线，另一组边线完全丢失或者只残留一点时条用
* char EdgeType:边线数组的类型
* int StartIndex:开始计算的数组下标
*/
void OnlyOneEdge(int StartIndex, int EndIndex ,char EdgeType, int Index)
{
	g_CenterNum = 0;  // 中心点个数
	int move = MAX_VIDEO_POINT / 2;   // 位移大小
	int LeftWeight = 1;  // 左线权重大小
	int RightWeight = 1;  // 右线条权重大小
	if (EdgeType == 'L')  // 只有左边线
	{
		int i = StartIndex;
		while ((g_LeftEdge[i].y + move) <= 161 && i < EndIndex)
		{
			g_CenterPosition[i].x = i;
			g_CenterPosition[i].y = g_LeftEdge[i].y + move * LeftWeight;
			g_CenterNum += 1;
			i++;
		}
	}
	if (EdgeType == 'R')  // 只有右边线
	{
		int i = StartIndex;
		while ((g_RightEdge[i].y - move) >= 0 && i < EndIndex)
		{
			g_CenterPosition[i].x = i;
			g_CenterPosition[i].y = g_RightEdge[i].y + move * RightWeight;
			g_CenterNum += 1;
			i++;
		}
	}
	if (EdgeType == 'l')
	{
		g_CenterPosition[Index].x = g_LeftEdge[Index].x;
		g_CenterPosition[Index].y = g_LeftEdge[Index].y;
		g_CenterNum += 1;
	}
	if (EdgeType == 'r')
	{
		g_CenterPosition[Index].x = g_RightEdge[Index].x;
		g_CenterPosition[Index].y = g_RightEdge[Index].y;
		g_CenterNum += 1;
	}
}

/*初始化值*/
void InitVariate()
{
	// 所有标志位需要复位
	FlagStraightLine = 0;  // 直道的标志，默认不是
	FlagCarage = 0;  // 车库标志，默认不是
	FlagOnlyLeftEdge = 0;   // 只有左边线
	FlagOnlyRightEdge = 0;  // 只有右边线
	FlagLeftToCarage = 0;
	FlagRightToCarage = 0;  // 左右入库
	FlagLeftOutCarage = 0;
	FlagRightOutCarage = 0;  // 左右出库
	FlagCross = 0;  // 十字

	g_LeftEdgeEndNum = 0;
	g_RightEdgeEndNum = 0;
	for (int i = 0; i < MAX_POINT_NUM; i++)
		g_CenterPosition[i].x = g_CenterPosition[i].y = g_LeftEdge[i].x = g_LeftEdge[i].y = g_RightEdge[i].x = g_RightEdge[i].y = 0;

}

/*正十字*/
void StraightCross(int flag)
{
	/*识别到完整的十字*/
	// 正十字
	_POINT InflectionPointLeftUp, InflectionPointLeftDown, InflectionPointRightUp, InflectionPointRightDown;  // 四个拐点坐标
	InflectionPointLeftDown.x = 0;
	InflectionPointLeftDown.y = 0;
	InflectionPointLeftUp.x = 0;
	InflectionPointLeftUp.y = 0;
	InflectionPointRightDown.x = 0;
	InflectionPointRightDown.y = 0;
	InflectionPointRightUp.x = 0;
	InflectionPointRightUp.y = 0;
	for (int i = 0; i < g_LeftEdgeNum - 1; i++)
	{
		if (g_LeftEdge[i].x != 0 && g_LeftEdge[i].y == 0 && g_LeftEdge[i + 1].x > 0 && g_LeftEdge[i + 1].y == 0)
		{
			InflectionPointLeftDown.x = g_LeftEdge[i].x;
			InflectionPointLeftDown.y = g_LeftEdge[i].y;
			break;
		}
	}
	for (int i = g_LeftEdgeNum - 2; i > 0; i--)
	{
		if (g_LeftEdge[i].x != 0 && g_LeftEdge[i].y == 0 && g_LeftEdge[i - 1].x != 0 && g_LeftEdge[i - 1].y > 0)
		{
			InflectionPointLeftUp.x = g_LeftEdge[i].x;
			InflectionPointLeftUp.y = g_LeftEdge[i].y;
			break;
		}
	}
	for (int i = 0; i < g_RightEdgeNum; i++)
	{
		if (g_RightEdge[i].x != 0 && g_RightEdge[i].y < 161 && g_RightEdge[i + 1].x >= 0 && g_RightEdge[i + 1].y == 161)
		{
			InflectionPointRightDown.x = g_RightEdge[i].x;
			InflectionPointRightDown.y = g_RightEdge[i].y;
			break;
		}
	}
	for (int i = g_RightEdgeNum - 2; i > 0; i--)
	{
		if ((g_RightEdge[i].x != 0 && g_RightEdge[i].y < 161) && (g_RightEdge[i - 1].x != 0 && g_RightEdge[i - 1].y == 161))
		{
			InflectionPointRightUp.x = g_RightEdge[i].x;
			InflectionPointRightUp.y = g_RightEdge[i].y;
			break;
		}
	}
	// 补线
	// 左边
	float LeftSlope = (InflectionPointLeftUp.y - InflectionPointLeftDown.y) / (InflectionPointLeftUp.x - InflectionPointLeftDown.x);
	float LeftIntercept = InflectionPointLeftDown.y - LeftSlope * InflectionPointLeftDown.x;
	for (int i = InflectionPointLeftDown.x; i < InflectionPointLeftUp.x; i++)
	{
		g_LeftEdge[i].x = i;
		g_LeftEdge[i].y = uint8(LeftSlope * i + LeftIntercept);
	}

	// 右边
	float RightSlope = (InflectionPointRightDown.y - InflectionPointRightUp.y) / (InflectionPointRightDown.x - InflectionPointRightUp.x);
	float RightIntercept = InflectionPointRightDown.y - RightSlope * InflectionPointRightDown.x;
	for (int i = InflectionPointRightDown.x; i < InflectionPointRightUp.x; i++)
	{
		g_RightEdge[i].x = i;
		g_RightEdge[i].y = uint8(RightSlope * i + RightIntercept);
	}
	
	// 中线，可以认为是一条直道
	StraightLine();
}

/*预处理*/
void PreDipose()
{
	g_LeftEdgeNum = 0;
	g_RightEdgeNum = 0;
	g_SearchFlag = 1;
	int i, line, white_width = 0;
	g_CenterNum = 0;
	// 搜边线
	for (line = 0; line <= MAX_VIDEO_LINE - 1; line++)
	{
		//首先 求出本行所有白块的位置
		g_SEnum = 0;  //本行的白块计数器清零
		if (g_VideoImageData[line][MAX_VIDEO_POINT - 1] > IMAGE_MIDDLE)  //大于阈值为白 小于阈值为黑
			g_Start[g_SEnum] = 0;
		for (i = MAX_VIDEO_POINT - 1; i > 0; i--)
		{
			if (g_VideoImageData[line][i] > IMAGE_MIDDLE && g_VideoImageData[line][i - 1] <= IMAGE_MIDDLE)
			{
				g_End[g_SEnum] = MAX_VIDEO_POINT - i;
				g_SEnum++;
			}
			else if (g_VideoImageData[line][i] <= IMAGE_MIDDLE && g_VideoImageData[line][i - 1] > IMAGE_MIDDLE)
			{
				g_Start[g_SEnum] = MAX_VIDEO_POINT - i - 1;
			}
			else if (g_SEnum >= 10)
			{
				break;
			}
		}
		if (g_VideoImageData[line][0] > IMAGE_MIDDLE && g_SEnum < 10)
		{
			g_End[g_SEnum] = MAX_VIDEO_POINT - 1;
			g_SEnum++;
		}
		//把最下面的那一行单独拿出来处理 做为后续处理的基础
		if (g_SearchFlag)                    //最下一行处理标志
		{
			int temp_mark = 0;
			if (g_SEnum == 0)
			{
				continue;
			}
			white_width = g_End[0] - g_Start[0];
			for (i = 1; i < g_SEnum; i++)  //直接求最宽的白块
			{
				if (g_End[i] - g_Start[i] >= white_width)
				{
					white_width = g_End[i] - g_Start[i];
					temp_mark = i;
				}
			}
			if (white_width > WIDTH)
			{
				g_SearchFlag = 0;
				//认为第一行最宽的白块便是所需白块
				g_LeftEdge[g_LeftEdgeNum].x = line;
				g_LeftEdge[g_LeftEdgeNum++].y = g_Start[temp_mark];
				g_RightEdge[g_RightEdgeNum].x = line;
				g_RightEdge[g_RightEdgeNum++].y = g_End[temp_mark];
			}
		}
		else
		{
			if (g_SEnum == 0)
			{
				break;
			}
			for (i = 0, g_CoverNum = 0; i < g_SEnum; i++)
			{
				//覆盖关系，因为图像是一个联通区域 于是找与上一行已经找到白块相联通的白块
				g_Cover = MIN(g_End[i], g_RightEdge[g_RightEdgeNum - 1].y) - MAX(g_Start[i], g_LeftEdge[g_LeftEdgeNum - 1].y);
				if (g_Cover > -1)
				{
					g_CoverIndex[g_CoverNum] = i;
					g_CoverNum++;
				}
			}
			if (g_CoverNum == 1) //如果只有一个联通的 直接取出这个联通白块
			{
				g_LeftEdge[g_LeftEdgeNum].x = line;
				g_LeftEdge[g_LeftEdgeNum++].y = g_Start[g_CoverIndex[0]];
				g_RightEdge[g_RightEdgeNum].x = line;
				g_RightEdge[g_RightEdgeNum++].y = g_End[g_CoverIndex[0]];
			}
			else if (g_CoverNum == 0)  //没有联通的白块 说明图像已经处理完毕
			{
				break;                 //跳出整个图像处理的循环 进入求中心部分
			}
			else //有多个白块   需要取舍
			{
				//取舍规则：先求出本行的白块的中心，再寻找与上一行白块中心最相近的一个白块
				int temp_mark, temp_dis, temp_center, last_center;
				temp_mark = g_CoverIndex[0];
				temp_center = (g_Start[g_CoverIndex[0]] + g_End[g_CoverIndex[0]]) / 2;
				last_center = (g_LeftEdge[g_LeftEdgeNum - 1].y + g_RightEdge[g_RightEdgeNum - 1].y) / 2;
				temp_dis = ABS(last_center - temp_center);
				for (i = 1; i < g_CoverNum; i++)
				{
					temp_center = (g_Start[g_CoverIndex[i]] + g_End[g_CoverIndex[i]]) / 2;
					if (ABS(temp_center - last_center) < temp_dis)
					{
						temp_dis = ABS(temp_center - last_center);
						temp_mark = i;
					}
				}
				g_LeftEdge[g_LeftEdgeNum].x = line;
				g_LeftEdge[g_LeftEdgeNum++].y = g_Start[temp_mark];
				g_RightEdge[g_RightEdgeNum].x = line;
				g_RightEdge[g_RightEdgeNum++].y = g_End[temp_mark];
			}
		}
	}

}